#include "Transform.h"
#include <GL/gl.h>
#include "utilities.h"


Transform::Transform(): scal(Vector3f(1,1,1))
{
}

Vector3f Transform::getTranslation() const
{
    return trans;
}

Vector3f Transform::getRotation() const
{
    return rot;
}

Vector3f Transform::getScale() const
{
    return scal;
}

void Transform::setTranslation(const Vector3f &vec)
{
    trans = vec;
}

void Transform::setScale(const Vector3f &vec)
{
    scal = vec;
}

void Transform::setRotation(const Vector3f &vec)
{
    rot = vec;
}

Transform &Transform::translate(const Vector3f &vec)
{
    trans = trans + vec;

//    //Concatenando com a matriz de translacao
//    for(int i = 0; i<4; i++){
//      m_matrix(i,3) += vec[i];
//    }


//    //Concatenando com a inversa
//    for(int i = 0; i<4; i++){
//     m_inv(i,3) += -vec[i];
//    }

    return *this;
}

Transform &Transform::scale(const Vector3f &vec)
{

    scal = scal * vec;

//    //Concatenando com a matriz
//    m_matrix(0,0) *= vec[0];
//    m_matrix(1,1) *= vec[1];
//    m_matrix(2,2) *= vec[2];

//    //Criando uma matriz m
//    Matrix4f m;
//    m(0,0) = vec[0];
//    m(1,1) = vec[1];
//    m(2,2) = vec[2];

//    //Concatenando a matriz inversa anterior a m
//    m_inv = m_inv*m;

    return *this;
}

Transform &Transform::rotateX(float ang)
{
    rot.x += ang;

//    //Criando uma matriz m1 generica
//    Matrix4f m1;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m1(0,0) = 0;
//        }
//    }

//    //Setando os elementos da matriz
//    m1(0,0) *= 1;
//    m1(1,1) *= cos(ang);
//    m1(1,2) *= -sin(ang);
//    m1(2,1) *= sin(ang);
//    m1(2,2) *= cos(ang);
//    m1(3,3) *= 1;

//    m_matrix = m_matrix*m1;

//    //Criando uma matriz m2 generica
//    Matrix4f m2;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m2(0,0) = 0;
//        }
//    }


//    //Setando os elementos da matriz inversa (-ang)
//    m2(0,0) *= 1;
//    m2(1,1) *= cos(-ang);
//    m2(1,2) *= -sin(-ang);
//    m2(2,1) *= sin(-ang);
//    m2(2,2) *= cos(-ang);
//    m2(3,3) *= 1;

//    //Multiplicando a matriz inversa atual com a matriz
//    m_inv = m_inv*m2;

    return *this;
}

Transform &Transform::rotateY(float ang)
{
    rot.y += ang;

//    //Criando uma matriz m1 generica
//    Matrix4f m1;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m1(0,0) = 0;
//        }
//    }

//    //Setando os elementos da matriz
//    m1(0,0) *= cos(ang);
//    m1(0,2) *= sin(ang);
//    m1(1,1) *= 1;
//    m1(2,0) *= -sin(ang);
//    m1(2,2) *= cos(ang);
//    m1(3,3) *= 1;

//    m_matrix = m_matrix*m1;

//    //Criando uma matriz m2 generica
//    Matrix4f m2;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m2(0,0) = 0;
//        }
//    }


//    //Setando os elementos da matriz inversa (-ang)
//    m2(0,0) *= cos(-ang);
//    m2(0,2) *= sin(-ang);
//    m2(1,1) *= 1;
//    m2(2,0) *= -sin(-ang);
//    m2(2,2) *= cos(-ang);
//    m2(3,3) *= 1;

//    //Multiplicando a matriz inversa atual com a matriz
//    m_inv = m_inv*m2;

    return *this;
}

Transform &Transform::rotateZ(float ang)
{
    rot.z += ang;

//    //Criando uma matriz m1 generica
//    Matrix4f m1;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m1(0,0) = 0;
//        }
//    }

//    //Setando os elementos da matriz
//    m1(0,0) *= cos(ang);
//    m1(0,1) *= -sin(ang);
//    m1(1,0) *= sin(ang);
//    m1(1,1) *= cos(ang);
//    m1(2,2) *= 1;
//    m1(3,3) *= 1;

//    m_matrix = m_matrix*m1;

//    //Criando uma matriz m2 generica
//    Matrix4f m2;
//    for(int i = 0; i< 4; i++){
//        for(int j = 0; j < 4; j++){
//            m2(0,0) = 0;
//        }
//    }


//    //Setando os elementos da matriz inversa (-ang)
//    m2(0,0) *= cos(-ang);
//    m2(0,1) *= -sin(-ang);
//    m2(1,0) *= sin(-ang);
//    m2(1,1) *= cos(-ang);
//    m2(2,2) *= 1;
//    m2(3,3) *= 1;

//    //Multiplicando a matriz inversa atual com a matriz
//    m_inv = m_inv*m2;

    return *this;
}

Transform Transform::operator *(const Transform &t) const
{
    Transform result;

    result.trans = trans + t.trans;
    result.rot = rot + t.rot;
    result.scal = scal * t.scal;

    return result;
}

Matrix4f Transform::matrix() const
{
    /*Rotacao*/

    //Fazendo a Rotacao X
    Matrix4f rotX;

    float cosAng = cos(RADIANS(rot.x));
    float sinAng = sin(RADIANS(rot.x));

    //Setando os elementos na Matriz
    rotX(1,1) = cosAng;
    rotX(1,2) = -sinAng;
    rotX(2,1) = sinAng;
    rotX(2,2) = cosAng;

    //Fazendo a rotacao Y
    Matrix4f rotY;

    cosAng = cos(RADIANS(rot.y));
    sinAng = sin(RADIANS(rot.y));

    //Setando os elementos da matriz
    rotY(0,0) = cosAng;
    rotY(0,2) = sinAng;
    rotY(2,0) = -sinAng;
    rotY(2,2) = cosAng;

    //Fazendo a rotacao Z
    Matrix4f rotZ;

    cosAng = cos(RADIANS(rot.z));
    sinAng = sin(RADIANS(rot.z));

    //Setando os elementos da matriz
    rotZ(0,0) = cosAng;
    rotZ(0,1) = -sinAng;
    rotZ(1,0) = sinAng;
    rotZ(1,1) = cosAng;

    /*Translacao*/
    Matrix4f transResult;

    //Concatenando com a matriz de translacao
    transResult(0,3) = trans[0];
    transResult(1,3) = trans[1];
    transResult(2,3) = trans[2];


    /*Escala */
    Matrix4f scaleResult;

    scaleResult(0,0) = scal[0];
    scaleResult(1,1) = scal[1];
    scaleResult(2,2) = scal[2];

    //Retornando a concantenacao das matrizes
    return transResult*scaleResult*rotZ*rotY*rotX;

}

Matrix4f Transform::invMatrix() const
{
    /*Rotacao*/

    //Fazendo a Rotacao X
    Matrix4f rotX;

    float cosAng = cos(RADIANS(-rot.x));
    float sinAng = sin(RADIANS(-rot.x));

    //Setando os elementos na Matriz
    rotX(1,1) = cosAng;
    rotX(1,2) = -sinAng;
    rotX(2,1) = sinAng;
    rotX(2,2) = cosAng;

    //Fazendo a rotacao Y
    Matrix4f rotY;

    cosAng = cos(RADIANS(-rot.y));
    sinAng = sin(RADIANS(-rot.y));

    //Setando os elementos da matriz
    rotY(0,0) = cosAng;
    rotY(0,2) = sinAng;
    rotY(2,0) = -sinAng;
    rotY(2,2) = cosAng;

    //Fazendo a rotacao Z
    Matrix4f rotZ;

    cosAng = cos(RADIANS(-rot.z));
    sinAng = sin(RADIANS(-rot.z));

    //Setando os elementos da matriz
    rotZ(0,0) = cosAng;
    rotZ(0,1) = -sinAng;
    rotZ(1,0) = sinAng;
    rotZ(1,1) = cosAng;

    /*Translacao*/
    Matrix4f transResult;

    //Concatenando com a matriz de translacao
    transResult(0,3) = -trans[0];
    transResult(1,3) = -trans[1];
    transResult(2,3) = -trans[2];


    /*Escala */
    Matrix4f scaleResult;

    scaleResult(0,0) = 1.f/scal[0];
    scaleResult(1,1) = 1.f/scal[1];
    scaleResult(2,2) = 1.f/scal[2];

    //Retornando a concantenacao das matrizes inversas
    return rotX*rotY*rotZ*scaleResult*transResult;
}

void Transform::applyGL() const
{

    glTranslatef(trans.x,trans.y,trans.z);

    glScaled(scal.x,scal.y,scal.z);

    glRotatef(rot.z,0,0,1);
    glRotatef(rot.y,0,1,0);
    glRotatef(rot.x,1,0,0);

}

void Transform::applyInverseGL()
{
    glRotatef(-rot.x,1,0,0);
    glRotatef(-rot.y,0,1,0);
    glRotatef(-rot.z,0,0,1);
    glScaled(1.f/scal.x,1.f/scal.y,1.f/scal.z);
    glTranslatef(-trans.x,-trans.y,-trans.z);
}
